/**
 * File: robot_control_node.cpp
 * Brief: this node is work for receive velocity institution from the 
 *        corresponding topic ,and transmit it to robot.
 * Created by: zhangping
 * Created at: 2017.06.6
 * Modified by: 2017.07.6
 * Modified at:
 **/

#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "protocol.h"
#include "communication.h"
#include "kinematic.h"

typedef const boost::function<void (const geometry_msgs::Twist::ConstPtr& msg)> InsCallback;


/**
 * This callback function will forward Institution message to our Robot.
 */
void InsForward(const geometry_msgs::Twist::ConstPtr& msg, Communication &com)
{
    ROS_INFO("Twist> [T=%f,W=%f]", msg->linear.x, msg->angular.z);
    U8 x[2] = {0}; // move at the x-axis of car, move right
    U8 y[2] = {0}; // move at the y-axis of car, move foward
    U8 z[2] = {0}; // rotation, 
    U8 orientation = 0;
    
    /* 注意：小车的x轴对应于Twist的y轴，小车的y轴对应于Twist的x轴。小车的y轴对应于前进速度。*/
    kinematic::U16 move_foward = kinematic::LinearVelocityToPulse(msg->linear.x);
    kinematic::U16 move_right = kinematic::LinearVelocityToPulse(msg->linear.y);
    kinematic::U16 rotation = kinematic::RotationVelocityToPluse(msg->angular.z);
    kinematic::U8  direction = kinematic::ComputeDirection(msg->linear.y
                                        , msg->linear.x, msg->angular.z);

    y[0] = move_foward/255;
    y[1] = move_foward%255;
    x[0] = move_right/255;
    x[1] = move_right%255;
    z[0] = rotation/255;
    z[1] = rotation%255;
    orientation = direction;
    
    ROS_INFO("Car> [ %x %x %x %x %x %x %x]", x[0], x[1]
                                           , y[0], y[1]
                                           , z[0], z[1]
                                           ,orientation);
	if(!com.SendCmdWithArgs( kPosMode, x, y ,z, orientation)){
		std::cerr<<"Error: Send Insititution failed"<<std::endl;
	}
}


int main(int argc, char **argv)
{
    /* Init Node */
    ros::init(argc, argv, "robot_control_node");
    ros::NodeHandle n;

    /* deal with Institution message. */
    Communication com("192.168.1.1",2001);
    InsCallback ins_callback = boost::bind(InsForward, _1, boost::ref(com));
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, ins_callback);

    /* wait */
    ros::spin();

    return 0;
}
